#include"ImageConsProd.h"
#include"ArmorDetector.h"
#include"ownh.h"
#include"Predictor.h"
#include"poseSolver.h"
#include"serialport.h"
#define ESC
#define USE_PORT
//#define USEJPG
using namespace std;
using namespace cv;
#define BUFFER_SIZE 1

volatile unsigned int prdIdx;
volatile unsigned int csmIdx;
volatile bool ending=0;
float x_initial_angle=0.0;
float y_initial_angle=0.0;

int mode=0;////////1==red,2==blue/////////
struct ImageData {
    Mat img;
    unsigned int frame;
};
ImageData imgdata[BUFFER_SIZE];//imgs live here 

void ImageConsProd::ImageProducer(){
#ifdef USE_PORT
    while(1){
        mode=_port.get_Mode();
        if(mode==1||mode==2)
            break;
    }
#else 
    mode=1;//not use port ,set this 
#endif
    VideoCapture cap(0); 
    cameraInit(cap);
    //////////////////////// Setting Camera End //////////////////////
    Mat img;
    while(1){
         int cnt = 2000;
        while(cnt--){

            while(prdIdx - csmIdx >= BUFFER_SIZE);

#ifdef USE_PORT
            if (_port.get_Mode() == 3){
                ending=1;
                exit(0);
            }
#endif  
            ///////here  need to add a exit autoshoot para
            // 开始读取图片
#ifdef USEJPG
                     cout<<"yes";
                     img=imread("/home/like/xbotcon/test.jpg");
#else
                if (cap.read(img)){

                }
                else
                { 
                
#ifdef ESC
                    char  q=waitKey(1);
                    if (q==27){
                        exit(0);
                    }
#endif
                    continue ;
                }
#endif
                imgdata[prdIdx % BUFFER_SIZE].img = img;
                ++prdIdx;
            }
    }
    return ;
}

void ImageConsProd::ImageConsumer(){
    
    Mat src;
    ArmorDetector a;
    poseSolver pose;
    Predictor pre;
    VisionData data;//接受角度和距离数据

    // =================== 大循环 =======================
    while(1){

        int cnt = 2000;
        while(cnt--)
        {
            if(ending){
                exit(0);
            }
        // 接收敌方颜色信息
            a.set_mode(mode);
            while(prdIdx - csmIdx == 0);
            
            Mat temp;
            imgdata[csmIdx % BUFFER_SIZE].img.copyTo(temp);
            a.set_frame(temp);
            ++csmIdx;
            
            a.GetArmor();
            if(a.find()){
                pose.solve_for_armor(a.finalvertex,0);
                if(pose.wrong_dis())
                    continue;
                else
                    {
                        pre.predictor(pose.get_yaw(),pose.get_pitch());
                        data.pitch_angle=pre.get_pitch()-y_initial_angle;
                        data.yaw_angle=pre.get_yaw()-x_initial_angle;
                        x_initial_angle=pre.get_yaw();
                        y_initial_angle=pre.get_pitch();
#ifdef USE_PORT         
                        _port.TransformData(data);
                        _port.send();
#endif                       
                    }
            }
#ifdef ESC
            char  q=waitKey(1);
            if (q==27){
                exit(0);
            }
#endif
        }
    }

    return ;
}